package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S0: minimizes curvature derivative, by making the control point
 * angles equal at a station.
 */
public class ForceS0 extends ForceSubsystem {

	ForceS0(ForceSystem system) {
		super(system);
	}

	void applyForces(int delta) {
		for (MetroLine l : system.metroMap.lines) {
			for (int i = 1; i < l.stations.length - 1; i++) {
				MetroStation m1 = l.stations[i - 1];
				MetroStation m2 = l.stations[i];
				MetroStation m3 = l.stations[i + 1];

				applyForces(m1, m2, m3);
			}

			// Detect cycle at end of line
			if (l.stations.length >= 3) {
				MetroStation m1 = l.stations[l.stations.length - 2];
				MetroStation m2 = l.stations[l.stations.length - 1];
				MetroStation m3 = null;
				for (int i = 0; i < l.stations.length - 1; i++) {
					if (l.stations[i].id == m2.id) {
						m3 = l.stations[i + 1];
						break;
					}
				}
				if (m3 != null) {
					applyForces(m1, m2, m3);
				}
			}
		}
	}

	private void applyForces(MetroStation m1, MetroStation m2, MetroStation m3) {
		MetroEdge e1 = system.metroMap.getEdge(m1.id, m2.id);
		MetroEdge e2 = system.metroMap.getEdge(m2.id, m3.id);

		Body b1 = system.controlPointBodies[e1.id];
		Body b2 = system.stationBodies[m2.id];
		Body b3 = system.controlPointBodies[e2.id];

		Vec2 p1 = b1.getPosition();
		Vec2 p2 = b2.getPosition();
		Vec2 p3 = b3.getPosition();

		Vec2 p21 = p1.sub(p2);
		Vec2 p23 = p3.sub(p2);

		double theta21 = Math.atan2(p21.y, p21.x); // between p21 and x-axis
		double theta23 = Math.atan2(p23.y, p23.x); // between p23 and x-axis

		// First rotate the vectors such that the vector from 2 to 1
		// lies on the x-axis. Then subtract Pi to set a straight line
		// (opposite vectors) to 0.
		double theta = (theta23 - theta21) - Math.PI;
		// Move the range to [-pi, pi).
		if (theta <= -Math.PI)
			theta += 2f * Math.PI;

		Vec2 f1 = new Vec2(-p21.y, p21.x);
		Vec2 f3 = new Vec2(p23.y, -p23.x);

		float len1 = f1.normalize();
		float len3 = f3.normalize();

		f1.mulLocal((float) theta * ForceConstants.S0_WEIGHT / len1);
		f3.mulLocal((float) theta * ForceConstants.S0_WEIGHT / len3);

		b1.applyForceToCenter(f1);
		b3.applyForceToCenter(f3);
	}
}
